{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center><img src=\"../image/CLBLOGO.jpg\" alt=\"创乐博\" style=\"width: 300px;\"/></center>\n",
    "\n",
    "# 10.OpenCV 摄像头云台物体追踪\n",
    "\n",
    "@－－－－湖南创乐博智能科技有限公司－－－－<br>\n",
    "@  文件名：10.OpenCV摄像头云台物体追踪.ipynb <br>\n",
    "@  版本：V2.0 <br>\n",
    "@  author: zhulin<br>\n",
    "@  说明：OpenCV摄像头云台物体追踪<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 为了可以打开摄像头，这里关闭摄像头所有进程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 载入必要的库\n",
    "import cv2\n",
    "import numpy as np\n",
    "from LOBOROBOT import LOBOROBOT # 载入机器人库\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "clbrobot = LOBOROBOT()  # 实例化机器人对象\n",
    "# Configure min and max servo pulse lengths\n",
    "servo_min = 150  # Min pulse length out of 4096\n",
    "servo_max = 600  # Max pulse length out of 4096"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 舵机调零\n",
    "pan =  90\n",
    "tilt = -10\n",
    "# 频率设置为50hz，适用于舵机系统。\n",
    "clbrobot.set_servo_angle(10,pan)  # 底座舵机 90 \n",
    "clbrobot.set_servo_angle(9,tilt)  # 顶部舵机 30\n",
    "time.sleep(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "c4010252c7ca4b5da042557f55900e5e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(Image(value=b'', format='jpeg', height='240', width='320'), Image(value=b'', format='jpeg', hei…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import traitlets\n",
    "import ipywidgets.widgets as widgets\n",
    "from IPython.display import display\n",
    "\n",
    "def bgr8_to_jpeg(value, quality=75):\n",
    "    return bytes(cv2.imencode('.jpg', value)[1])\n",
    "\n",
    "FGmaskComp_img = widgets.Image(format='jpeg', width=320, height=240)\n",
    "frame_img = widgets.Image(format='jpeg', width=320, height=240)\n",
    "\n",
    "dispaly_img = widgets.HBox([FGmaskComp_img,frame_img])\n",
    "display(dispaly_img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 线程函数操作库\n",
    "import threading # 线程\n",
    "import ctypes\n",
    "import inspect\n",
    "\n",
    "# 线程结束代码\n",
    "def _async_raise(tid, exctype):\n",
    "    tid = ctypes.c_long(tid)\n",
    "    if not inspect.isclass(exctype):\n",
    "        exctype = type(exctype)\n",
    "    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))\n",
    "    if res == 0:\n",
    "        raise ValueError(\"invalid thread id\")\n",
    "    elif res != 1:\n",
    "        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)\n",
    "        raise SystemError(\"PyThreadState_SetAsyncExc failed\")\n",
    "        \n",
    "def stop_thread(thread):\n",
    "    _async_raise(thread.ident, SystemExit)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "width: 320.0 height: 240.0\n"
     ]
    }
   ],
   "source": [
    "dispW=320\n",
    "dispH=240\n",
    "\n",
    "cam = cv2.VideoCapture(0,cv2.CAP_V4L2)\n",
    "cam.set(3,320)\n",
    "cam.set(4,240)\n",
    "cam.isOpened()\n",
    "\n",
    "width=cam.get(cv2.CAP_PROP_FRAME_WIDTH)\n",
    "height=cam.get(cv2.CAP_PROP_FRAME_HEIGHT)\n",
    "print('width:',width,'height:',height)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "fdf8c60f68dd4701ae716c84fd749f62",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "VBox(children=(HBox(children=(IntSlider(value=96, description='hueLower：', max=179, min=96), IntSlider(value=1…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "hueLower = widgets.IntSlider(min=96,max=179,step=1,description='hueLower：',value=96)\n",
    "hueUpper = widgets.IntSlider(min=120,max=179,step=1,description='hueUpper：',value=120)\n",
    "\n",
    "hue2Lower = widgets.IntSlider(min=50,max=179,step=1,description='hue2Lower：',value=50)\n",
    "hue2Upper = widgets.IntSlider(min=0,max=179,step=1,description='hue2Upper：',value=0)\n",
    "\n",
    "satLow = widgets.IntSlider(min=157,max=255,step=1,description='satLow：',value=157)\n",
    "satHigh = widgets.IntSlider(min=255,max=255,step=1,description='satHigh：',value=255)\n",
    "\n",
    "valLow = widgets.IntSlider(min=100,max=255,step=1,description='valLow：',value=100)\n",
    "valHigh = widgets.IntSlider(min=255,max=255,step=1,description='valHigh：',value=255)\n",
    "\n",
    "slider_img=widgets.VBox([\n",
    "              widgets.HBox([hueLower,hueUpper]),\n",
    "              widgets.HBox([hue2Lower,hue2Upper]),\n",
    "              widgets.HBox([satLow,satHigh]),\n",
    "              widgets.HBox([valLow,valHigh])\n",
    "             ])\n",
    "display(slider_img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Video_display():\n",
    "    global pan\n",
    "    global tilt\n",
    "    while True:   \n",
    "        ret, frame = cam.read()\n",
    "        frame=cv2.flip(frame, 0)\n",
    "        hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)\n",
    "\n",
    "        hueLow=hueLower.value \n",
    "        hueUp=hueUpper.value\n",
    "\n",
    "        hue2Low=hue2Lower.value\n",
    "        hue2Up=hue2Upper.value\n",
    "\n",
    "        Ls= satLow.value\n",
    "        Us = satHigh.value\n",
    "\n",
    "        Lv=valLow.value\n",
    "        Uv=valHigh.value\n",
    "\n",
    "        l_b=np.array([hueLow,Ls,Lv])\n",
    "        u_b=np.array([hueUp,Us,Uv])\n",
    "\n",
    "        l_b2=np.array([hue2Low,Ls,Lv])\n",
    "        u_b2=np.array([hue2Up,Us,Uv])\n",
    "\n",
    "        FGmask=cv2.inRange(hsv,l_b,u_b)\n",
    "        FGmask2=cv2.inRange(hsv,l_b2,u_b2)\n",
    "        FGmaskComp=cv2.add(FGmask,FGmask2)\n",
    "        FGmaskComp_img.value = bgr8_to_jpeg(FGmaskComp)\n",
    "\n",
    "        contours,_=cv2.findContours(FGmaskComp,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)\n",
    "        contours=sorted(contours,key=lambda x:cv2.contourArea(x),reverse=True)\n",
    "\n",
    "        for cnt in contours:\n",
    "            area=cv2.contourArea(cnt)\n",
    "            (x,y,w,h)=cv2.boundingRect(cnt)\n",
    "            if area>=50:\n",
    "                cv2.rectangle(frame,(x,y),(x+w,y+h),(255,0,0),3)\n",
    "                objX=x+w/2\n",
    "                objY=y+h/2\n",
    "\n",
    "                errorPan=objX-width/2\n",
    "                errorTilt=objY-height/2            \n",
    "\n",
    "                if abs(errorPan)>15:\n",
    "                    pan=pan-errorPan/75    \n",
    "\n",
    "                if abs(errorTilt)>15:\n",
    "                    tilt=tilt-errorTilt/75\n",
    "\n",
    "                if pan>90:\n",
    "                    pan=90\n",
    "                    print(\"Pan Out of  Range\") \n",
    "                    #clbrobot.set_servo_angle(10,90)\n",
    "                if pan<-90:\n",
    "                    pan=90\n",
    "                    print(\"Pan Out of  Range\")              \n",
    "                    #clbrobot.set_servo_angle(10,90)\n",
    "                if tilt>90:\n",
    "                    tilt=90\n",
    "                    print(\"Tilt Out of  Range\") \n",
    "                if tilt<-90:\n",
    "                    tilt=90\n",
    "                    print(\"Tilt Out of  Range\")\n",
    "                    \n",
    "                clbrobot.set_servo_angle(10,90-pan)\n",
    "                clbrobot.set_servo_angle(9,90-tilt) \n",
    "                break \n",
    "            else:\n",
    "                clbrobot.set_servo_angle(10,90)  # 底座舵机 90 \n",
    "                clbrobot.set_servo_angle(9,0)  # 顶部舵机 30\n",
    "                \n",
    "        frame_img.value = bgr8_to_jpeg(frame)\n",
    "    cam.release()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n",
      "Tilt Out of  Range\n"
     ]
    }
   ],
   "source": [
    "t = threading.Thread(target=Video_display)\n",
    "t.setDaemon(True)\n",
    "t.start()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 结束线程\n",
    "stop_thread(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
